#include "Encoder.hpp"

uint16_t cc_irq = 0;
volatile int16_t Cir = 0;
extern volatile uint8_t flag;
#ifdef CH32
extern "C" void TIM1_CC_IRQHandler(void) __attribute__((interrupt("WCH-Interrupt-fast")));
extern "C" void TIM1_UP_IRQHandler(void) __attribute__((interrupt("WCH-Interrupt-fast")));
void TIM1_CC_IRQHandler(void)
{
    if (TIM_GetITStatus(TIM1, TIM_IT_CC3) != RESET)
    {
        cc_irq = TIM1->CNT;
        flag = 1;
        TIM_ClearITPendingBit(TIM1, TIM_IT_CC3);
    }
}
// #include "USB.hpp"
void TIM1_UP_IRQHandler(void)
{
    if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET)
    {
        // cc_irq = TIM1->CNT;
        // USB_Printf("%d %d\n",Cir,TIM1->CNT);
        if (TIM1->CNT < 500)
        {
            Cir++;
        }
        else if (TIM1->CNT > 16000)
        {
            Cir--;
        }
        TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
    }
}
Encoder::Encoder(uint16_t value)
{
    ENCOEDR_CPR = value;
    DEGTrans = 2 * PI / ENCOEDR_CPR * 4;
    engle_offset = 0;
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    NVIC_InitTypeDef NVIC_InitStructure;
    TIM_ICInitTypeDef TIM_ICInitStructure;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    TIM_DeInit(TIM1);
    TIM_TimeBaseStructure.TIM_Prescaler = 0;
    TIM_TimeBaseStructure.TIM_Period = value - 1;
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
    TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
    TIM_EncoderInterfaceConfig(TIM1, TIM_EncoderMode_TI12, TIM_ICPolarity_Falling, TIM_ICPolarity_Rising);
    TIM_ICStructInit(&TIM_ICInitStructure); // 初始化为默认值
    TIM_ICInitStructure.TIM_Channel = TIM_Channel_3;
    TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
    TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
    TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
    TIM_ICInitStructure.TIM_ICFilter = 5;
    TIM_ICInit(TIM1, &TIM_ICInitStructure);
    NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannel = TIM1_UP_IRQn;
    NVIC_Init(&NVIC_InitStructure);
    TIM1->CNT = 0;
}

Encoder::~Encoder()
{
    RCC_APB2PeriphResetCmd(RCC_APB2Periph_TIM1, ENABLE);
}
#endif
#ifdef AT32
extern "C" void TMR1_CH_IRQHandler(void)
{
    if (tmr_flag_get(TMR1, TMR_C3_FLAG) != RESET)
    {
        cc_irq = TMR1->cval;
        flag = 1;
        tmr_flag_clear(TMR1, TMR_C3_FLAG);
    }
}

extern "C" void TMR1_OVF_TMR10_IRQHandler(void)
{
    if (tmr_flag_get(TMR1, TMR_OVF_FLAG) != RESET)
    {
        if (TMR1->cval < 500)
        {
            Cir++;
        }
        else if (TMR1->cval > 16000)
        {
            Cir--;
        }
        tmr_flag_clear(TMR1, TMR_OVF_FLAG);
    }
}
Encoder::Encoder(uint16_t value)
{
    ENCOEDR_CPR = value;
    DEGTrans = 2 * PI / ENCOEDR_CPR * 4;
    engle_offset = 0;
    crr_periph_clock_enable(CRR_TMR1_PERIPH_CLOCK, TRUE);
    tmr_input_config_type tmr_input_config_struct;
    tmr_base_init(TMR1, ENCOEDR_CPR, 0);
    tmr_cnt_dir_set(TMR1, TMR_COUNT_UP);
    /* config encoder mode */
    tmr_encoder_mode_config(TMR1, TMR_ENCODER_MODE_C, TMR_INPUT_FALLING_EDGE, TMR_INPUT_RISING_EDGE);
    /* configure tmr3 channel2 to get clock signal */
    tmr_input_config_struct.input_channel_select = TMR_SELECT_CHANNEL_3;
    tmr_input_config_struct.input_mapped_select = TMR_CC_CHANNEL_MAPPED_DIRECT;
    tmr_input_config_struct.input_polarity_select = TMR_INPUT_RISING_EDGE;
    tmr_input_config_struct.input_filter_value = 3;
    tmr_input_channel_init(TMR1, &tmr_input_config_struct, TMR_CHANNEL_INPUT_DIV_1);
    tmr_interrupt_enable(TMR1, TMR_C3_INT, TRUE);
    tmr_interrupt_enable(TMR1, TMR_OVF_INT, TRUE);
    /* tmr2 trigger interrupt nvic init */
    nvic_irq_enable(TMR1_CH_IRQn, 13, 0);
    nvic_irq_enable(TMR1_OVF_TMR10_IRQn, 12, 0);
    /* enable tmr1 */
}

Encoder::~Encoder()
{
    crr_periph_reset(CRR_TMR1_PERIPH_RESET, TRUE);
}
#endif
#ifdef STM32
extern "C" void TIM1_UP_TIM10_IRQHandler(void)
{
    if (__HAL_TIM_GET_FLAG(&htim1, TIM_FLAG_UPDATE) != RESET)
    {
        if (__HAL_TIM_GET_IT_SOURCE(&htim1, TIM_IT_UPDATE) != RESET)
        {
            if (TIM1->CNT < 500)
            {
                Cir++;
            }
            else if (TIM1->CNT > 16000)
            {
                Cir--;
            }
            __HAL_TIM_CLEAR_IT(&htim1, TIM_IT_UPDATE);
        }
    }
}
extern "C" void TIM1_CC_IRQHandler(void)
{
    if (__HAL_TIM_GET_FLAG(&htim1, TIM_FLAG_CC3) != RESET)
    {
        if (__HAL_TIM_GET_IT_SOURCE(&htim1, TIM_IT_CC3) != RESET)
        {
            {
                cc_irq = TIM1->CNT;
                flag = 1;
                __HAL_TIM_CLEAR_IT(&htim1, TIM_IT_CC3);
            }
        }
    }
}
Encoder::Encoder(uint16_t value)
{
    ENCOEDR_CPR = value;
    DEGTrans = 2 * PI / ENCOEDR_CPR * 4;
    engle_offset = 0;
    MX_TIM1_Init(ENCOEDR_CPR);
}

Encoder::~Encoder()
{
    __HAL_RCC_TIM1_CLK_DISABLE();
}
#endif
int32_t Encoder::getRawEncoder()
{
#ifdef CH32
    return TIM1->CNT + Cir * ENCOEDR_CPR;
#endif
#ifdef STM32
    return TIM1->CNT + Cir * ENCOEDR_CPR;
#endif
#ifdef AT32
    return TMR1->cval + Cir * ENCOEDR_CPR;
#endif
}

int32_t Encoder::getRawEncoder(bool value)
{
#ifdef CH32
    return TIM1->CNT;
#endif
#ifdef STM32
    return TIM1->CNT;
#endif
#ifdef AT32
    return TMR1->cval;
#endif
}

float Encoder::getDegEncoder()
{
    switch (F_MODE)
    {
    case 0:
        return getRawEncoder() * DEGTrans;
        break;
    case 1:
        return encoder_ += speed_;
        break;
    default:
        return 0;
        break;
    }
}
void Encoder::setMode(uint8_t setvalue)
{
    F_MODE = setvalue;
}

void Encoder::setSpeed(float speed)
{
    speed_ = speed;
}

float Encoder::getEngle_offset()
{
    return engle_offset;
}
/**
 * @brief 设置角度偏移
 *
 * @param value 最大2PI 最小-2PI
 */
void Encoder::setEngle_offset(float value)
{
    engle_offset = value;
}

float Encoder::getRotspeed()
{
    time = Time::getTime();
    pos = getRawEncoder();
    float temp = ((pos - last_pos) / (time - last_time)) / 16384;
    last_pos = pos;
    last_time = time;
    return temp;
}